#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include "friend_layer_setting.pb.h"
#include "friend_layer.h"
#include<math.h>
#include "tf/transform_datatypes.h"
#include <nav_msgs/Odometry.h>

 
using roborts_costmap::LETHAL_OBSTACLE;
using roborts_costmap::NO_INFORMATION;
using roborts_costmap::FREE_SPACE;
 
namespace roborts_costmap{

    FriendLayer::FriendLayer(std::string map_name):map_name_(map_name) {   

        flag = 0;
        friend_area_map.header.frame_id = "/map";
        friend_area_map.point.x;
        friend_area_map.point.y;
        
        if(map_name_ == "global_costmap"){
            friend_area = friend_area_map;
        }
    }



    void FriendLayer::OnInitialize(){

        ros::NodeHandle nh("~/" + name_),g_nh;
    
        ParaFriendLayer  para_friendlayer;

        std::string friend_layer = ros::package::getPath("roborts_costmap") + \
        "/config/friend_layer_config.prototxt";
        roborts_common::ReadProtoFromTextFile(friend_layer.c_str(), &para_friendlayer);
        length =para_friendlayer.len();
        width =para_friendlayer.wid();
        friend_area = friend_area_map;

        is_current_ = true;
        is_enabled_ = true;
        default_value_ = NO_INFORMATION;
        MatchSize();
        flayer_sub_ = nh.subscribe("/air_block", 10,&FriendLayer::FlayerInfoCallback,this);
    }

    void FriendLayer::FlayerInfoCallback(const geometry_msgs::PoseStamped :: ConstPtr&msg){
        friend_area.point.x = msg->pose.position.x;
        friend_area.point.y = msg->pose.position.y;
    
        tf::Quaternion quat;
        tf::quaternionMsgToTF(msg->pose.orientation, quat);
        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
    } 

    void FriendLayer::MatchSize(){

        Costmap2D* master = layered_costmap_->GetCostMap();
        ResizeMap(master->GetSizeXCell(), master->GetSizeYCell(), master->GetResolution(),master->GetOriginX(), master->GetOriginY());
    }

    void FriendLayer::UpdateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y){

        if (!is_enabled_)
            return;
        if (flag == 0){
            flag = 1;
        }
        else{
            return;
        }
        double mark_x = 1;
        double mark_y = 1;
        *min_x = std::min(*min_x, mark_x);
        *min_y = std::min(*min_y, mark_y);
        *max_x = std::max(*max_x, mark_x);
        *max_y = std::max(*max_y, mark_y);
    }
  
    void FriendLayer::UpdateCosts(roborts_costmap::Costmap2D& master_new, int min_i, int min_j, int max_i, int max_j){
        unsigned int mx;
        unsigned int my;
        double mark_x ,mark_y; 
        double base_x,base_y;
        base_x = layered_costmap_->GetCostMap()->GetOriginX();
        base_y = layered_costmap_->GetCostMap()->GetOriginY();
        double fposx[5]={0,0,0,0,0};
        double fposy[5]={0,0,0,0,0};
        if (!is_enabled_)
        return;
        for(auto cell:friend_cells){
            master_new. SetCost(cell.first,cell.second,FREE_SPACE);
        }
        mark_x = friend_area.point.x -base_x;
        mark_y = friend_area.point.y -base_y;
        //mark_x=3.07;
        //mark_y=1.89;
        //yaw=5;
        fposx[0]=mark_x+length*sin(yaw)+width*cos(yaw);
        fposy[0]=mark_y+length*cos(yaw)-width*sin(yaw);

        fposx[1]=mark_x+length*sin(yaw)-width*cos(yaw);
        fposy[1]=mark_y+length*cos(yaw)+width*sin(yaw);

        fposx[2]=mark_x-length*sin(yaw)-width*cos(yaw);
        fposy[2]=mark_y-length*cos(yaw)+width*sin(yaw);

        fposx[3]=mark_x-length*sin(yaw)+width*cos(yaw);
        fposy[3]=mark_y-length*cos(yaw)-width*sin(yaw);

        fposx[4]= fposx[0];
        fposy[4]= fposy[0];

       double posx,posy;

        for(int k=0;k<4;k++){
            for(int a=0;a<=15;a++){
                int b = 15-a;
                posx=(a*fposx[k]+b*fposx[k+1])/15;
                posy=(a*fposy[k]+b*fposy[k+1])/15;
                if(World2Map(posx, posy, mx, my)){
                     //std::cout<<"mx="<<mx<<"  my="<<my<<std::endl;
                    master_new.SetCost(mx,  my,LETHAL_OBSTACLE);
                }
            }
        }
    }
}
